#include "CyclicCurvePopulation.h"

namespace cagd
{

    CyclicCurvePopulation::Individual::Individual(GLuint n, ColumnMatrix<DCoordinate3>& data_point_to_interpolate, GLuint max_order_of_derivatives, GLuint div_point_count):
            _n(n),
            _data_point_to_interpolate(data_point_to_interpolate),
            _max_order_of_derivatives(max_order_of_derivatives),
            _div_point_count(div_point_count),
            _individual(new CyclicCurve3(n))
    {
        randomizeKnots(2 * n + 1);

        // One Curve
        //        _knots_vector.ResizeRows(2 * _n + 1);
        //        GLdouble step = 2.0 * PI / (2 * _n + 1);
        //        for (GLuint i = 0; i <= 2 * _n; ++i)
        //        {
        //            _knots_vector[i] = i * step;
        //        }

        //        cout << _knots_vector << endl;

        render();

    }

    CyclicCurvePopulation::Individual::Individual(GLuint n, ColumnMatrix<DCoordinate3>& data_point_to_interpolate,
                                                  ColumnMatrix<GLdouble> knot_vector, GLuint max_order_of_derivatives,
                                                  GLuint div_point_count):
            _n(n),
            _data_point_to_interpolate(data_point_to_interpolate),
            _knots_vector(knot_vector),
            _max_order_of_derivatives(max_order_of_derivatives),
            _div_point_count(div_point_count),
            _individual(new CyclicCurve3(n))
    {
//        _individual = new CyclicCurve3(n);
        render();
    }

    GLvoid CyclicCurvePopulation::Individual::render() {
        if (!_individual->UpdateDataForInterpolation(_knots_vector, _data_point_to_interpolate)) {
            throw Exception("Could not solve the curve interpolation problem!");
        }
        else
        {
            _fitness = _individual->Length(100);
            if (!_individual->UpdateVertexBufferObjectsOfData())
            {
                throw Exception("Could not generate the control polygon of the interpolating cyclic curve!");
            }

            _image = _individual->GenerateImage(_max_order_of_derivatives, _div_point_count);
            if (!_image)
            {
                throw Exception("Could not generate the image of the interpolating cyclic curve!");
            }
            else
            {
                if (!_image->UpdateVertexBufferObjects())
                {
                    throw Exception("Could not generate the vertex buffer object of the interpolating curve's image!");
                }
            }
        }
    }

    GLboolean CyclicCurvePopulation::Individual::randomizeKnots(GLuint number_of_knots) {
        if (!number_of_knots) {
            _knots_vector.ResizeColumns(0);
            return GL_FALSE;
        }
        _knots_vector.ResizeRows(number_of_knots);

        GLdouble step = 2.0 * PI / (number_of_knots);

        for (GLuint i = 0; i < number_of_knots; ++i)
        {
            _knots_vector[i] = i * step;
        }

        for (GLuint i = 1; i < number_of_knots - 1; i++) {
            GLdouble v = (GLdouble)rand() / (GLdouble)RAND_MAX;           // [0 ..   1]
            GLdouble scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);     // [0 ..   1]


            GLdouble r_max = 0.9;
            while (scale > r_max) {                                 // force [0 .. r_max] interval
                scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);    // [0 ..   1]
            }

            if (v < 0.5) {
                _knots_vector[i] -= (_knots_vector[i] - _knots_vector[i-1]) * scale;
            } else {
                _knots_vector[i] += (_knots_vector[i+1] - _knots_vector[i]) * scale;
            }

            DCoordinate3 &ref = (*_individual)[i];

            ref[0] = 1.0 * cos(_knots_vector[i]);
            ref[1] = 1.0 * sin(_knots_vector[i]);
            ref[2] = 0.0; //-2.0 + 4.0 * (GLdouble)rand() / (GLdouble)RAND_MAX;
        }

        //        cout << "Knot vector of Individual: " << _knots_vector << endl;

        return GL_TRUE;
    }

    GLboolean CyclicCurvePopulation::Individual::calculateLength() {
        _fitness = _individual->Length(100);
        return GL_TRUE;
    }

    CyclicCurvePopulation::CyclicCurvePopulation(
            GLuint number_of_individuals, GLuint n,
            ColumnMatrix<DCoordinate3> data_point_to_interpolate,
            GLuint iterationBetweenGeneration,
            GLuint max_order_of_derivatives, GLuint div_point_count):
            _n(n),
            _data_point_to_interpolate(data_point_to_interpolate),
            _iterationBetweenGeneration(iterationBetweenGeneration),
            _number_of_individuals(number_of_individuals)
    {
        r_max = 0.9;
        if (!number_of_individuals) {
            _individuals.ResizeColumns(0);
        }

        _individuals.ResizeColumns(number_of_individuals);

        for (GLuint i = 0; i < number_of_individuals; i++) {
            _individuals[i] = new Individual(n,_data_point_to_interpolate,max_order_of_derivatives,div_point_count);
        }

        sort();
        for (GLuint i = 0; i < number_of_individuals; i++) {
            cout << _individuals[i]->getFitness() << endl;
        }
    }

    GLvoid CyclicCurvePopulation::sort() {
        Individual* temp;
        GLuint exchange;
        for(GLuint i = 0; i < _number_of_individuals; i++){
            exchange = 0;
            for(GLuint j = 0; j < _number_of_individuals - 1; j++){
                if(_individuals[j]->getFitness() > _individuals[j+1]->getFitness()) {
                    temp = _individuals[j];
                    _individuals[j] = _individuals[j+1];
                    _individuals[j+1] = temp;
                    exchange++;
                    //                    cout<<"\n arr[j] = "<<arr[j]<<"  arr[j+1] = "<<arr[j+1];
                }
            }
            //            cout<<endl;
            if(exchange == 0)
                break;
//            else
//                cout << "EXCHANGE\n";
        }
    }

    //    GLvoid CyclicCurvePopulation::setSelectionSize() {
    //        _selection_size = 0;
    //        for (GLuint i = 1; i <= _number_of_individuals; i++) {
    //            _selection_size += i;
    //        }
    //        _selection.ResizeColumns(_selection_size);

    //        GLuint x = 0;
    //        GLuint pos = 0;
    //        for (GLuint i = _number_of_individuals; i > 0; i--) {
    //            for (GLuint j = 0; j < i; j++) {
    //                _selection[pos++] = x;
    //            }
    //            x++;
    //        }
    //    }

    CyclicCurvePopulation::Individual* CyclicCurvePopulation::getBest() {
        return _individuals[0];
    }

    RowMatrix<CyclicCurvePopulation::Individual*> CyclicCurvePopulation::getAllIndividual() {
        return _individuals;
    }

    CyclicCurvePopulation::Individual* CyclicCurvePopulation::selection(RowMatrix<Individual*> _individuals) {
        GLuint index = rand() % _number_of_individuals;
//        cout << _individuals[1]->getFitness() << endl;
        return _individuals[index];
    }

    GLboolean CyclicCurvePopulation::mutation() {
        Individual* actual = selection(_individuals);

        // [1 ..   knot_nr - 2] <- because indexes are between [0 .. knor_nr -1] but first and last we doesn`t want to change
        int i = rand() % (actual->_knots_vector.GetRowCount() - 2) + 1;
        GLdouble v = (GLdouble)rand() / (GLdouble)RAND_MAX;           // [0 ..   1]
        GLdouble scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);     // [0 ..   1]

        while (scale > r_max) {                                 // force [0 .. r_max] interval
            scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);    // [0 ..   1]
        }

        GLdouble diff;

        if (v < 0.5) {
            // i and i-1
            diff = actual->_knots_vector[i] - actual->_knots_vector[i-1];
            actual->_knots_vector[i] -= (diff) * scale;
            GLdouble old_length = actual->getFitness();
            actual->render();

            if (actual->getFitness() > old_length) {
                actual->_knots_vector[i] += (diff) * scale;
                actual->render();

                return GL_FALSE;
            }
        } else {
            // i and i+1
            diff = actual->_knots_vector[i+1] - actual->_knots_vector[i];
            actual->_knots_vector[i] += (diff) * scale;
            GLdouble old_length = actual->getFitness();
            actual->render();

            if (actual->getFitness() > old_length) {
                actual->_knots_vector(i) -= (diff) * scale;
                actual->render();

                return GL_FALSE;
            }
        }

        return GL_TRUE;
    }

    ColumnMatrix<GLdouble> CyclicCurvePopulation::sortKnots(ColumnMatrix<GLdouble> knots_vector) {
        GLdouble temp;
        GLuint exchange;
        for(GLuint i = 1; i < knots_vector.GetRowCount() - 2; i++){
            exchange = 0;
            for(GLuint j = 1; j < knots_vector.GetRowCount() - 3; j++){
                if(knots_vector[j] > knots_vector[j+1]) {
                    temp = knots_vector[j];
                    knots_vector[j] = knots_vector[j+1];
                    knots_vector[j+1] = temp;
                    exchange++;
                    //                    cout<<"\n arr[j] = "<<arr[j]<<"  arr[j+1] = "<<arr[j+1];
                } else if (knots_vector[j] == knots_vector[j+1]) {
                    knots_vector[j+1] += 0.000001;
                }
            }
            //            cout<<endl;
            if(exchange == 0)
                break;
        }
        return knots_vector;
    }

    GLboolean CyclicCurvePopulation::recombination() {
        Individual* ind_first = selection(_individuals);
        Individual* ind_second = selection(_individuals);
        while (ind_first == ind_second)
            ind_second = selection(_individuals);

        GLuint knot_index = rand() % (ind_first->_knots_vector.GetRowCount() - 2) + 1; // middle: [1 ... knotSize -1]

//        cout << knot_index << endl;

        ColumnMatrix<GLdouble> child_first_knots;
        ColumnMatrix<GLdouble> child_second_knots;
        child_first_knots.ResizeRows(ind_first->_knots_vector.GetRowCount());
        child_second_knots.ResizeRows(ind_first->_knots_vector.GetRowCount());

        for (GLuint i = 0; i < knot_index; i++) {
            child_first_knots[i] = ind_first->_knots_vector[i];
            child_second_knots[i] = ind_second->_knots_vector[i];
        }

        for (GLuint i = knot_index; i < ind_first->_knots_vector.GetRowCount(); i++) {
            child_first_knots[i] = ind_second->_knots_vector[i];
            child_second_knots[i] = ind_first->_knots_vector[i];
        }

        child_first_knots = sortKnots(child_first_knots);
        child_second_knots = sortKnots(child_second_knots);

        Individual* child_first = new Individual(_n,_data_point_to_interpolate,child_first_knots);
        Individual* child_second = new Individual(_n,_data_point_to_interpolate,child_second_knots);

//        for (GLuint i = 0; i < _number_of_individuals; i++) {
//            cout << _individuals[i]->getFitness() << " ";
//        }

//        cout << endl;

//        cout << "first: " << ind_first->getFitness();
//        cout << " second: " << ind_second->getFitness();
//        cout << " childfirst: " << child_first->getFitness();
//        cout << " childsecond: " << child_second->getFitness() << endl;

//        for (GLuint i = 0; i < _number_of_individuals; i++) {
//            cout << _individuals[i]->getFitness() << " ";
//        }

//        cout << endl;

        searchPlace(ind_first,ind_second,child_first);
        searchPlace(ind_first,ind_second,child_second);

        sort();

        return GL_TRUE;
    }

    GLvoid CyclicCurvePopulation::searchPlace(Individual* first_parent, Individual* second_parent, Individual* child) {
        if (child->getFitness() < first_parent->getFitness()) {
            first_parent = child;
        } else {
            if (child->getFitness() < second_parent->getFitness()) {
                second_parent = child;
            } else {
                if (child->getFitness() < _individuals[_individuals.GetColumnCount()-2]->getFitness()) {
                    _individuals[_individuals.GetColumnCount()-2] = child;
                } else {
                    if (child->getFitness() < _individuals[_individuals.GetColumnCount()-1]->getFitness()) {
                        _individuals[_individuals.GetColumnCount()-1] = child;
                    }
                }
            }
        }
    }

    GLvoid CyclicCurvePopulation::evolve() {
        GLdouble mutation_nr = 0.7;
        for (GLuint i = 0; i < _iterationBetweenGeneration; i++) {
            GLdouble r = (GLdouble)rand() / RAND_MAX;
            if (r <= mutation_nr) {
                mutation();
            } else {
                recombination();
            }
        }
    }

}
